/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "hungarianoper.h"
#include <vector>
#include <utility>

namespace airos {
namespace perception {
namespace algorithm {

Eigen::Matrix<float, -1, 2, Eigen::RowMajor> HungarianOper::Solve(
    const DYNAMICM &cost_matrix) {
  int rows = cost_matrix.rows();
  int cols = cost_matrix.cols();
  Matrix<double> matrix(rows, cols);
  for (int row = 0; row < rows; row++) {
    for (int col = 0; col < cols; col++) {
      matrix(row, col) = cost_matrix(row, col);
    }
  }
  // Munkres get matrix;
  Munkres<double> m;
  m.solve(matrix);

  //
  std::vector<std::pair<int, int>> pairs;
  for (int row = 0; row < rows; row++) {
    for (int col = 0; col < cols; col++) {
      int tmp = static_cast<int>(matrix(row, col));
      if (tmp == 0) pairs.push_back(std::make_pair(row, col));
    }
  }
  //
  int count = pairs.size();
  Eigen::Matrix<float, -1, 2, Eigen::RowMajor> re(count, 2);
  for (int i = 0; i < count; i++) {
    re(i, 0) = pairs[i].first;
    re(i, 1) = pairs[i].second;
  }
  return re;
}  // end Solve;

}  // namespace algorithm
}  // namespace perception
}  // namespace airos
